#ifndef KINCO_HPP
#define KINCO_HPP

#include "dispatch.h"
#include "can.hpp"
#include <iostream>
#include <string>
#include <math.h>

namespace CANalyst2
{
    #define DEC_TO_AP_DIV   20.48
    #define DEC_TO_RMS_DIV  28.96
    #define ACC_DEC_MULT    0.016384
    #define RPM_TO_DEC_MULT 0.273067
    struct __attribute__((__packed__)) MotorCtrlCmd
    {
        uint16_t ctrl;
        int32_t target;
    };

    struct __attribute__((__packed__)) MotorStatusFbk
    {
        uint16_t status;
        int32_t pos;
        int16_t current;
    };

    enum Servo_Mode {
        IMMDE_VEL_MODE = -3,
        POS_MODE = 1,
        VEL_MODE = 3,
        TORQUE_MODE = 4,
        HOME_MODE = 6,
    };

    enum Servo_Status {
        INVALID,
        SWITCH_ON_DISABLED,
        READY_TO_SWITCH_ON,
        SWITCHED_ON,
        OPERATION_ENABLED,
        QUICK_STOP_ACT,
        FAULT_REACTION_ACT,
        FAULT,
    };

    class Kinco 
    {
    private:
        uint8_t slave_id = 1;
        int can_ch = 0;
        bool is_pos_mode;
        int resolution;
        uint32_t profiled_vel;
        uint32_t profiled_acc;
        uint32_t profiled_dec;
        bool _is_fault;
        bool is_motor_on = false;
        uint16_t _error_code;
        uint16_t _error_cnt;
        int _error_req;

        MotorStatusFbk status_fbk;
        Servo_Status current_status = Servo_Status::INVALID;

        std::shared_ptr<CANDispatch> can;
        int processRecvTPDOFrame(const CanStamped &recv_frame);
        int processRecvEmcyFrame(const CanStamped &recv_frame);
        void parseStatusWord(uint16_t status);

    public:
        Kinco(std::shared_ptr<CANDispatch> _can, int ch, int _slave_id, bool _is_pos_mode = true, int _resolution = 65536, 
                uint32_t _profiled_vel = 200, uint32_t _profiled_acc = 200, uint32_t _profiled_dec = 200);
        ~Kinco();
 
        int SendEnableMotorPDO();
        int SendDisableMotorPDO();
        void Setup();
        void SendMoveHomeSDO();
        int32_t SendMoveTargetAbsSDO(float pos);
        int32_t SendMoveTargetRelSDO(float pos);

        bool SendSetWorkModeSDO(Servo_Mode mode);
        bool SendSetTargetVelocitySDO(int32_t rpm);
        bool SendSetProfiledVelocitySDO(uint32_t rpm);
        bool SendSetProfiledVelocityPDO(uint32_t rpm);
        bool SendSetProfiledAccSDO(uint32_t rps);
        bool SendSetProfiledDecSDO(uint32_t rps);

        void SendAcquireValidWorkModeSDO(int8_t &mode, bool &is_acq);
        void SendAcquireServoWorkModeSDO(int8_t &mode, bool &is_acq);
        void SendAcquireStatusWordSDO(uint16_t &status, bool &is_acq);
        void SendAcquireCtrlWordSDO(uint16_t &ctrl, bool &is_acq);
        void SendAcquireTargetPosSDO(int32_t &pos, bool &is_acq);
        void SendAcquireActualPosSDO(int32_t &pos, bool &is_acq);
        void SendAcquireTargetVelocitySDO(int32_t &vel, bool &is_acq);
        void SendAcquireActualVelocitySDO(int32_t &vel, bool &is_acq);
        void SendAcquireProfiledVelSDO(uint32_t &acc, bool &is_acq);
        void SendAcquireProfiledAccSDO(uint32_t &acc, bool &is_acq);
        void SendAcquireProfiledDecSDO(uint32_t &dec, bool &is_acq);
        void SendAcquireActualCurrentSDO(int16_t current, bool &is_acq);
        

        void SendMoveTargetPosPDO(float pos);
        void SendMoveTargetPosPDO(float pos, uint32_t rpm);

        void SendSyncFrame();
        void SendSetPosModeSDO();
        void SendSetHomeModeSDO();
        float GetCurPosPDO();
        MotorStatusFbk GetCurFBKPDO(float &pos, float &ap, float &rms);
        Servo_Status GetCurStatusPDO();
        int16_t GetCurrentPDO(float &ap, float &rms);
        inline bool GetMode() { return is_pos_mode; }
        inline void SetMode(bool is_pos) { is_pos_mode = is_pos; }
        inline uint16_t GetErrorCode() { return _error_code; }
    };
}

#endif